{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# In this example we will show the difference between fixes computed with laika\n",
    "# from raw data of the ublox receiver vs the the fixes the ublox receiver\n",
    "# computes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "with open('example_data/raw_gnss_ublox/t', 'rb') as f:\n",
    "  raw_ublox_t = np.load(f)\n",
    "with open('example_data/raw_gnss_ublox/value', 'rb') as f:\n",
    "  raw_ublox = np.load(f)\n",
    "with open('example_data/live_gnss_ublox/t', 'rb') as f:\n",
    "  fixes_ublox_t = np.load(f)\n",
    "with open('example_data/live_gnss_ublox/value', 'rb') as f:\n",
    "  fixes_ublox = np.load(f)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We get the raw data into our format from the log array format\n",
    "\n",
    "from laika.raw_gnss import normal_meas_from_array\n",
    "measurements = np.array([normal_meas_from_array(arr) for arr in raw_ublox])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# initialize an astrodog with dgps corrections\n",
    "\n",
    "from laika import AstroDog\n",
    "dog = AstroDog(dgps=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'/tmp/gnss/cors_coord/cors_station_positions'"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Building this cache takes forever just copy it from repo\n",
    "\n",
    "from shutil import copyfile\n",
    "import os\n",
    "cache_directory = '/tmp/gnss/cors_coord/'\n",
    "try:\n",
    "  os.mkdir('/tmp/gnss/')\n",
    "except:\n",
    "  pass\n",
    "try:\n",
    "  os.mkdir(cache_directory)\n",
    "except:\n",
    "  pass\n",
    "copyfile('cors_station_positions', cache_directory + 'cors_station_positions')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "No orbit data found for prn : G04 flagging as bad\n",
      "No orbit data found for prn : R06 flagging as bad\n",
      "No orbit data found for prn : R07 flagging as bad\n",
      "No orbit data found for prn : R12 flagging as bad\n",
      "No orbit data found for prn : R25 flagging as bad\n",
      "No orbit data found for prn : R27 flagging as bad\n",
      "No orbit data found for prn : R28 flagging as bad\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "\r",
      "  0%|          | 0/600 [00:00<?, ?it/s]"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "pulling from http://ftpcache.comma.life/geodesy-noaa-gov/cors/rinex/2018/214/pbl1/pbl12140.18o.gz to /tmp/gnss/cors_obs/2018/214/pbl1/pbl12140.18o\n",
      "cache download failed, pulling from ftp://geodesy.noaa.gov/cors/rinex/2018/214/pbl1/pbl12140.18o.gz to /tmp/gnss/cors_obs/2018/214/pbl1/pbl12140.18o\n",
      "pulling from http://ftpcache.comma.life/geodesy-noaa-gov/cors/rinex/2018/214/pbl2/pbl22140.18o.gz to /tmp/gnss/cors_obs/2018/214/pbl2/pbl22140.18o\n",
      "cache download failed, pulling from ftp://geodesy.noaa.gov/cors/rinex/2018/214/pbl2/pbl22140.18o.gz to /tmp/gnss/cors_obs/2018/214/pbl2/pbl22140.18o\n",
      "pulling from http://ftpcache.comma.life/geodesy-noaa-gov/cors/rinex/2018/214/hsib/hsib2140.18o.gz to /tmp/gnss/cors_obs/2018/214/hsib/hsib2140.18o\n",
      "cache download failed, pulling from ftp://geodesy.noaa.gov/cors/rinex/2018/214/hsib/hsib2140.18o.gz to /tmp/gnss/cors_obs/2018/214/hsib/hsib2140.18o\n",
      "No dcb data found for prn : R26 flagging as bad\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 600/600 [01:23<00:00,  7.20it/s] \n"
     ]
    }
   ],
   "source": [
    "from laika.raw_gnss import process_measurements, correct_measurements, calc_pos_fix\n",
    "from tqdm import tqdm\n",
    "\n",
    "# We want to group measurements by measurement epoch\n",
    "# this makes the kalman filter faster and is easier\n",
    "# to reason about\n",
    "grouped_t = sorted(list(set(raw_ublox_t)))                                                                                      \n",
    "grouped_meas_processed = []\n",
    "corrected_meas_arrays = []\n",
    "\n",
    "# process measurement groups\n",
    "for t in grouped_t:\n",
    "  meas = measurements[raw_ublox_t == t]\n",
    "  grouped_meas_processed.append(process_measurements(meas, dog))\n",
    "\n",
    "# correct measurement groups with an estimate position\n",
    "# that was computes with weighted-least-squares on\n",
    "# the first epoch\n",
    "# WARNING: can take up to 10min\n",
    "wls_estimate = calc_pos_fix(grouped_meas_processed[0])\n",
    "est_pos = wls_estimate[0][:3]\n",
    "for proc in tqdm(grouped_meas_processed):\n",
    "  corrected = correct_measurements(proc, est_pos, dog)\n",
    "  corrected_meas_arrays.append(np.array([c.as_array() for c in corrected]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 600/600 [00:04<00:00, 125.11it/s]\n"
     ]
    }
   ],
   "source": [
    "for proc in tqdm(grouped_meas_processed):\n",
    "  corrected = correct_measurements(proc, est_pos, dog)\n",
    "  corrected_meas_arrays.append(np.array([c.as_array() for c in corrected]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "ename": "ModuleNotFoundError",
     "evalue": "No module named 'gnss'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-2-744c422db1fc>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      3\u001b[0m \u001b[0;32mfrom\u001b[0m \u001b[0mlaika_repo\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mexamples\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mkalman\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mgnss_kf\u001b[0m \u001b[0;32mimport\u001b[0m \u001b[0mGNSSKalman\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      4\u001b[0m \u001b[0;32mfrom\u001b[0m \u001b[0mlaika_repo\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mexamples\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mkalman\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mkalman_helpers\u001b[0m \u001b[0;32mimport\u001b[0m \u001b[0mrun_car_ekf_offline\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mObservationKind\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 5\u001b[0;31m \u001b[0mekf\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mGNSSKalman\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m      6\u001b[0m \u001b[0minit_state\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mekf\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mx\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      7\u001b[0m \u001b[0minit_state\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;36m3\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mest_pos\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m~/one/laika_repo/examples/kalman/gnss_kf.py\u001b[0m in \u001b[0;36m__init__\u001b[0;34m(self, N, max_tracks)\u001b[0m\n\u001b[1;32m     60\u001b[0m     \u001b[0mname\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m'gnss'\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     61\u001b[0m     \u001b[0;31m# init filter\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 62\u001b[0;31m     \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mfilter\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mEKF_sym\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mname\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mQ\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mx_initial\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mP_initial\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mdim_state\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mdim_state\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mmaha_test_kinds\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mmaha_test_kinds\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     63\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     64\u001b[0m   \u001b[0;34m@\u001b[0m\u001b[0mproperty\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m~/one/laika_repo/examples/kalman/ekf_sym.py\u001b[0m in \u001b[0;36m__init__\u001b[0;34m(self, name, Q, x_initial, P_initial, dim_main, dim_main_err, N, dim_augment, dim_augment_err, maha_test_kinds)\u001b[0m\n\u001b[1;32m    177\u001b[0m     \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0minit_state\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mx_initial\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mP_initial\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;32mNone\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    178\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 179\u001b[0;31m     \u001b[0mffi\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mlib\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mwrap_compiled\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mname\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mEXTERNAL_PATH\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    180\u001b[0m     \u001b[0mkinds\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mfeature_track_kinds\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m[\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    181\u001b[0m     \u001b[0;32mfor\u001b[0m \u001b[0mfunc\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mdir\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mlib\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m~/one/laika_repo/examples/kalman/ffi_wrapper.py\u001b[0m in \u001b[0;36mwrap_compiled\u001b[0;34m(name, directory)\u001b[0m\n\u001b[1;32m     39\u001b[0m \u001b[0;32mdef\u001b[0m \u001b[0mwrap_compiled\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mname\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mdirectory\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     40\u001b[0m   \u001b[0msys\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpath\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mappend\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdirectory\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 41\u001b[0;31m   \u001b[0mmod\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0m__import__\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mname\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     42\u001b[0m   \u001b[0;32mreturn\u001b[0m \u001b[0mmod\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mffi\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mmod\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mlib\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'gnss'"
     ]
    }
   ],
   "source": [
    "# We run the kalman filter\n",
    "\n",
    "from laika_repo.examples.kalman.gnss_kf import GNSSKalman\n",
    "from laika_repo.examples.kalman.kalman_helpers import run_car_ekf_offline, ObservationKind\n",
    "ekf = GNSSKalman()\n",
    "init_state = ekf.x\n",
    "init_state[:3] = est_pos\n",
    "ekf.init_state(init_state)\n",
    "ekf_data = {}\n",
    "ekf_data[ObservationKind.PSEUDORANGE_GPS] = (grouped_t, corrected_meas_arrays)\n",
    "ekf_data[ObservationKind.PSEUDORANGE_RATE_GPS] = (grouped_t, corrected_meas_arrays)\n",
    "ekf_outputs = run_car_ekf_offline(ekf, ekf_data)\n",
    "\n",
    "import laika.lib.coordinates as coord\n",
    "laika_positions_t = ekf_outputs[4]\n",
    "laika_positions_ecef = ekf_outputs[0][:,:3]\n",
    "laika_positions_geodetic = coord.ecef2geodetic(laika_positions_ecef)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ublox_positions_geodetic = fixes_ublox[:,[0,1,4]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# By looking at the map, we can see that the two paths compared.\n",
    "# If you want to regenerate the gmplot you will need a google\n",
    "# maps API key\n",
    "\n",
    "import gmplot\n",
    "gmap = gmplot.GoogleMapPlotter(*laika_positions_geodetic[0])\n",
    "#gmap.apikey='...'\n",
    "gmap.plot([x[0]  for x in laika_positions_geodetic], [x[1] for x in laika_positions_geodetic], 'blue', edge_width = 5)\n",
    "gmap.plot([x[0]  for x in ublox_positions_geodetic], [x[1] for x in ublox_positions_geodetic], 'red', edge_width = 5)\n",
    "gmap.draw(\"laika_quality_check.html\")\n",
    "\n",
    "\n",
    "\n",
    "import webbrowser\n",
    "import os\n",
    "webbrowser.open('file://' + os.path.realpath(\"laika_quality_check.html\"));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
